import numpy as np
import matplotlib.pyplot as plt
from matplotlib.widgets import Slider
from geometry import Vector, Quaternion
from fix import Projection
import math

def rotate2d(vec, bias = [0,0]):
    v_bia = np.matrix([bias[0], bias[1], 0]).astype(np.float32)
    vec = vec + v_bia
    vec /= np.linalg.norm(vec)
    return vec

def getQuaternion(vec, vbase):
    d = np.linalg.norm(vec)
    dbase = np.linalg.norm(vbase)
    vcross = np.cross(vbase, vec)
    len_cross = np.linalg.norm(vcross)
    if len_cross != 0:
        vcross /= len_cross
        vsin = len_cross / d / dbase
        vcos = math.sqrt(1 - vsin * vsin)
        vsin_half = math.sqrt((1 - vcos) / 2)
        vcos_half = math.sqrt((1 + vcos) / 2)
        return Quaternion(vcos_half, vsin_half * vcross[0][0], vsin_half * vcross[0][1], vsin_half * vcross[0][2])
    else:
        return Quaternion()


def update_all(val):
    sides = slider_sides.val
    try:
        h = slider_h.val
        tilt_x = slider_x.val
        tilt_y = slider_y.val
        shape_update(sides, h, tilt_x, tilt_y)
    except:
        shape_update(sides)


def shape_update(sides, h = 3, tilt_x = 0, tilt_y = 0):
    sides = int(sides)
    theta = np.linspace(-np.pi / sides, (2 - 1 / sides) * np.pi, sides + 1)

    # 骨架图形，防止matplotlib的坐标轴自适应地变化
    fx = np.array([2,-2,-2, 2, 2, 2,-2,-2, 2, 2])
    fy = np.array([2, 2,-2,-2, 2, 2, 2,-2,-2, 2])
    fz = np.array([4, 4, 4, 4, 4, 0, 0, 0, 0, 0])
    
    x = np.cos(theta)
    y = np.sin(theta)
    ox = np.cos(theta)
    oy = np.sin(theta)
    z = np.ones_like(x)
    z0 = np.zeros_like(x)

    # get attitude quaternion
    v1 = np.matrix([0,0,1])
    v2 = rotate2d(v1,[tilt_x, tilt_y])
    quat = getQuaternion(v1, v2)

    for i in range(sides + 1):
        vbase = Vector(x[i], y[i], 0)
        vout = Quaternion.rotate(quat, vbase)
        x[i] = vout.x
        y[i] = vout.y
        z[i] = vout.z + h

    ax.clear()
    ax.plot(fx, fy, fz, 'y', label='3D Curve')
    ax.plot(ox, oy, z0, 'g', label='3D Curve')

    x = np.append(x, [0])
    y = np.append(y, [0])
    z = np.append(z, [h])
    ax.plot(x, y, z, 'b', label='3D Curve')

    if not pj == None:
        pj.pre_transform(quat, h)
        # print('==============================')
        # print(x , y)
        pjx = []
        pjy = []
        pjz = []
        for i in range(sides + 1):
            pos = pj.transform([ox[i]/1.6, oy[i]/1.6], 2)
            pjx.append(pos[0])
            pjx.append(0)
            pjx.append(pos[0])
            pjy.append(pos[1])
            pjy.append(0)
            pjy.append(pos[1])
            pjz.append(0)
            pjz.append(h)
            pjz.append(0)
            print(pos)

        # print(x , y)

        ax.plot(pjx,pjy,pjz, 'r', label='3D Curve')


    return quat

    

if __name__ == '__main__':

    # 绘制3D图形
    fig = plt.figure()
    ax = fig.add_subplot(111, projection='3d')

    pj = None
    quat = shape_update(4)
    pj = Projection(quat)

    # 添加标签
    ax.set_title('Projection transform')
    ax.set_xlabel('X Label')
    ax.set_ylabel('Y Label')
    ax.set_zlabel('Z Label')
    ax.set_aspect('equal')
    ax.legend()

    # 设置交互式模式
    # ax.view_init(30, 185)
    ax.view_init(0, 0)

    plt.subplots_adjust(bottom = 0.2, top = 0.97)
    s1 = plt.axes([0.25, 0.15, 0.6, 0.05], facecolor='yellow') #设置slider1的位置
    slider_sides = Slider(s1, 'sides', valmin = 3, valmax = 20, valinit = 4 ,valstep = 1)#设置滑动条的属性
    slider_sides.on_changed(update_all)
    slider_sides.reset()#Reset the slider to the initial value.
    slider_sides.set_val(4)#Set slider value to val.

    s2 = plt.axes([0.25, 0.1, 0.6, 0.05], facecolor='yellow') #设置slider1的位置
    slider_h = Slider(s2, 'height', valmin = 0.1, valmax = 3.2, valinit = 1.6 ,valstep = 0.1)#设置滑动条的属性
    slider_h.on_changed(update_all)
    slider_h.reset()#Reset the slider to the initial value.
    slider_h.set_val(1.6)#Set slider value to val.

    s3 = plt.axes([0.25, 0.05, 0.6, 0.05], facecolor='blue') #设置slider1的位置
    slider_x = Slider(s3, 'x', valmin = -1.0, valmax = 1.0, valinit = 0.0 ,valstep = 0.05)#设置滑动条的属性
    slider_x.on_changed(update_all)
    slider_x.reset()#Reset the slider to the initial value.
    slider_x.set_val(0.0)#Set slider value to val.

    s4 = plt.axes([0.25, 0.0, 0.6, 0.05], facecolor='blue') #设置slider1的位置
    slider_y = Slider(s4, 'y', valmin = -1.0, valmax = 1.0, valinit = 0.35 ,valstep = 0.05)#设置滑动条的属性
    slider_y.on_changed(update_all)
    slider_y.reset()#Reset the slider to the initial value.
    slider_y.set_val(0.35)#Set slider value to val.

    plt.show()
